import numpy as np
import cv2
from show_map import map_img
import random

MAPPATH = "map.csv"

def csvpos2mappos(csvpos):
    mappos = [(csvpos[0]-1)/2, (csvpos[1]-1)/2]
    return mappos

def mappos2csvpos(mappos):
    csvpos = [mappos[0]*2+1, mappos[1]*2+1]
    return csvpos

def mappos2realpos(mappos):
    realpos = [60*(mappos[0]-2),-60*(mappos[1]-2)]
    return realpos

def read_map(filename="map.csv"):
    h=open(filename)
    H=h.read()
    Hh=H.split("\n")
    data=[]
    for row in Hh:
        data.append(row.split(","))
    return data

class Drone:
    def __init__(self, pos, direction):
        self.pos = pos
        self.direction = direction
    
    def forward(self):
        if self.direction == 0:
            self.pos[1]-=1
        elif self.direction == 1:
            self.pos[0]+=1
        elif self.direction == 2:
            self.pos[1]+=1
        elif self.direction == 3:
            self.pos[0]-=1
        #self.gotoxy(self.pos)
        
    
    def right(self):
        self.direction = (self.direction+1)%4
        #self.drone.right()
    
    def left(self):
        self.direction = (self.direction-1)%4
        #self.drone.left()

    def gotoxy(self, pos):
        self.pos = pos

class Map():
    def __init__(self, mArray):
        self.mArray = mArray


class Env():
    def __init__(self):
        self.drone = Drone(pos=[2,4], direction = 0)
        self.map = Map(mArray=read_map())
    
    def droneisblocked(self):
        pos = mappos2csvpos(self.drone.pos)
        direction = self.drone.direction
        if direction == 0:
            pos[1]-=1
        elif direction == 1:
            pos[0]+=1
        elif direction == 2:
            pos[1]+=1
        elif direction == 3:
            pos[0]-=1
        print(pos)
        return self.map.mArray[pos[1]][pos[0]] in ('1', '0')

e = Env()
img = map_img(e.drone.pos, e.drone.direction)
cv2.imshow("img", img)
cv2.waitKey(0)
step = 0
while True:
    step+=1
    print(step)
    e.drone.direction = random.randint(0, 3)
    if e.droneisblocked():
        print("blocked", e.drone.direction)
    else:
        print("pass", e.drone.direction)
        e.drone.forward()
    img = map_img(e.drone.pos, e.drone.direction)
    cv2.imshow("img", img)
    cv2.waitKey(10)
    if e.drone.pos == [2, 0]:
        break
print('ok')
